#include "Entity.hpp"

/** \file Entity.cpp
 *  \author Adrian Schweizer
 *  \created  $Do 30 Aug 10:36:45 pm CEST 2007 schwadri@SchwadriComp.local$
 *  \modified $So 28 Okt 04:56:24 pm CET 2007 schwadri@SchwadriLaptop$
 */

using namespace world;

Entity::Entity()
:   m_transform(1,0,0,0,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1)
{ }

Entity::Entity(real mass, const matrix3r& inertia, const vector3r& x0, const quatr& p0, const vector3r& xd0, const vector3r& o)
:   m_mass(mass),
    m_inertia(inertia),
    m_inertia_inv(inverse(inertia)),
    m_x(x0),
    m_p(p0),
    m_xdot(xd0),
    m_omega(o),
    m_h(0),
    m_torque(0),
    m_transform(1,0,0,0,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1)
{
    UpdateTransform();
}

Entity::Entity(real mass, const matrix3r& inertia)
:   m_mass(mass),
    m_inertia(inertia),
    m_inertia_inv(inverse(inertia)),
    m_x(0),
    m_p(1.0,0.0,0.0,0.0),       //identity mapping
    m_xdot(0),
    m_omega(0.0,0.0,0.0),
    m_h(0),
    m_torque(0),
    m_transform(1,0,0,0,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1)
{
    UpdateTransform();
}

void Entity::Inertia(const matrix3r& i)
{
    m_inertia       =   i;
    m_inertia_inv   =   inverse(i);
}

void Entity::Act(const vector3r& f, const vector3r& poa)
{
    m_h         += f;
    m_torque    += cross(poa,f);
}

void Entity::Act(const vector6r& d, const vector3r& poa)
{
    for(int i=0;i<3;++i)
        m_h[i]  +=  d[i];

    for(int i=0;i<3;++i)
        m_torque += d[i+3];

    //cross(poa,f)
    /*
    m_torque[3] +=  poa[1] * d[2] - poa[2] * d[1];
    m_torque[4] +=  poa[2] * d[0] - poa[0] * d[2];
    m_torque[5] +=  poa[0] * d[1] - poa[1] * d[0];
    */
    //FIXME: dirty hack. remove when vector subranges are implemented
    m_torque += cross(poa,reinterpret_cast<const vector3r&>(d));
}

void Entity::Act(const vector3r& f)
{
    m_h         += f;
}

void Entity::Act(const vector6r& d)
{
    for(int i=0;i<3;++i)
        m_h[i]  +=  d[i];

    for(int i=0;i<3;++i)
        m_torque += d[i+3];
}

void Entity::UpdateTransform()
{
    matrix3r r =    core::math::quaternion_to_matrix(m_p);
    m_inertia_inv_i = r * m_inertia_inv * transpose(r);

    for(int i=0;i<3;++i)
        for(int j=0;j<3;++j)
            m_transform[i][j] = r[i][j];

    for(int i=0;i<3;++i)
        m_transform[i][3]    =   m_x[i];
}

void Entity::Update(real dt)
{
    //calculate linear acceleration
    vector3r xddot = m_h / m_mass;
    //velocity integration
    m_xdot += xddot * dt;

    //reset force accumulator
    m_h = 0;

    //account for precession effects
    m_torque -= cross(m_omega,m_inertia_inv_i*m_omega);

    //calculate angular acceleration and update angular velocity quaternion
    m_omega += m_inertia_inv * m_torque;
    quatr pdot = quatr(0.0,m_omega[0],m_omega[1],m_omega[2])*m_p;//*0.5;
    pdot *= 0.5;

    //reset torque accumulator
    m_torque = 0;

    m_x += m_xdot * dt;
    m_p += pdot * dt;
    //normalize quaternion
    m_p /= norm(m_p);

    UpdateTransform();
}
